#include "aos/kernel.h"
#include <aos/mbmaster.h>
#include "modbus.h"
#include "aos/cli.h"

#define RECV_LEN_MAX       20
#define QUEUE_SIZE_MAX     (20)
#define MODBUS_TASK_DEPTH 2048

static aos_queue_t md_info_queue;
static md_dev_t md_info_buf[QUEUE_SIZE_MAX];
static mb_handler_t *mb_handler;
static int16_t write_buf[10];

static int modbus_busy = 0;

int modbus_read(uint16_t addr, uint16_t reg, uint16_t readlen, uint8_t *buf, uint32_t *reslen)
{
	if (NULL == md_info_queue.hdl ||
		NULL == buf ||
		NULL == reslen)
		return -1;
	md_dev_t md_rd_buf;
	md_rd_buf.rw_bit = MD_READ_D;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = reg;
	md_rd_buf.rwlen = readlen;
	md_rd_buf.reslen = reslen;
	md_rd_buf.resbuf = buf;
	
	return aos_queue_send(&md_info_queue, &md_rd_buf, sizeof(md_rd_buf));
}

int modbus_write(uint16_t addr, uint16_t reg, uint16_t writelen, uint16_t *wtbuf, uint8_t *buf, uint32_t *reslen)
{
	if (NULL == md_info_queue.hdl ||
		NULL == buf ||
		NULL == reslen)
		return -1;
	if (writelen > MAX_RW_REG_NUM){
		writelen = MAX_RW_REG_NUM;
		printf("warning:write len %d > %d\r\n", writelen, MAX_RW_REG_NUM);
	}
	md_dev_t md_rd_buf;
	md_rd_buf.rw_bit = MD_WRITE_D;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = reg;
	md_rd_buf.rwlen = writelen;
	md_rd_buf.reslen = reslen;
	md_rd_buf.resbuf = buf;
	md_rd_buf.wbuf = write_buf;
	memcpy(md_rd_buf.wbuf, wtbuf, writelen * 2);
	
	return aos_queue_send(&md_info_queue, &md_rd_buf, sizeof(md_rd_buf));
}

int modbus_get(uint16_t addr, uint16_t reg, uint16_t readlen, uint8_t *buf, uint32_t *reslen)
{
	if (NULL == md_info_queue.hdl ||
		NULL == buf ||
		NULL == reslen)
		return -1;
	int      status;
	md_dev_t md_rd_buf;
	md_rd_buf.rw_bit = MD_COIL_G;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = reg;
	md_rd_buf.rwlen = readlen;
	md_rd_buf.reslen = reslen;
	md_rd_buf.resbuf = buf;
	
	return aos_queue_send(&md_info_queue, &md_rd_buf, sizeof(md_rd_buf));
}

int modbus_set(uint16_t addr, uint16_t reg, uint16_t val, uint8_t *buf, uint32_t *reslen)
{
	if (NULL == md_info_queue.hdl ||
		NULL == buf ||
		NULL == reslen)
		return -1;
	md_dev_t md_rd_buf;
	md_rd_buf.rw_bit = MD_COIL_S;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = reg;
	md_rd_buf.rwlen = val;
	md_rd_buf.reslen = reslen;
	md_rd_buf.resbuf = buf;
	
	return aos_queue_send(&md_info_queue, &md_rd_buf, sizeof(md_rd_buf));
}

int modbus_opt(md_dev_t *md_buf)
{
	int status;

	if (NULL == md_buf)
		return -1;

	if (modbus_busy){
		return -1;
	}
	modbus_busy = 1;
	if (md_buf->rw_bit == MD_WRITE_D){  

		status = aos_mbmaster_write_holding_reginster(mb_handler, md_buf->dev_id, md_buf->reg_id, 
												md_buf->rwlen, md_buf->wbuf, md_buf->resbuf, md_buf->reslen);
	} else if (md_buf->rw_bit == MD_READ_D) {
		status = aos_mbmaster_read_holding_reginster(mb_handler, md_buf->dev_id, md_buf->reg_id, 
												md_buf->rwlen, md_buf->resbuf, md_buf->reslen);
	} else if (md_buf->rw_bit == MD_COIL_S) {
		status = aos_mbmaster_write_coil_reginster(mb_handler, md_buf->dev_id, md_buf->reg_id, 
												md_buf->rwlen, md_buf->resbuf, md_buf->reslen);
	} else if (md_buf->rw_bit == MD_COIL_G) {
		status = aos_mbmaster_read_coil_reginster(mb_handler, md_buf->dev_id, md_buf->reg_id, 
												md_buf->rwlen, md_buf->resbuf, md_buf->reslen);
	}
	aos_msleep(7);
	modbus_busy = 0;
	return status;
}

int16_t get_reg(uint16_t addr, uint16_t reg)
{
	uint8_t buf[10];
	uint32_t len = 0;
	md_dev_t md_rd_buf;
	int16_t reg_val = 0;

	md_rd_buf.rw_bit = MD_READ_D;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = reg;
	md_rd_buf.rwlen = 1;
	md_rd_buf.reslen = &len;
	md_rd_buf.resbuf = buf;
	if (!modbus_status()){
		if (modbus_opt(&md_rd_buf) < 0){
			return -1;
		}
		if (1 == len){
			error_handle(buf[0]);
			return -1;
		}
		reg_val = (buf[0] << 8) | buf[1]; 
		return reg_val;
	}
	return -1;
}

int16_t set_reg(uint16_t addr, uint16_t reg, int16_t val)
{
	uint8_t buf[10];
	uint32_t len = 0;
	int16_t num = 0;
	md_dev_t md_rd_buf;

	md_rd_buf.rw_bit = MD_WRITE_D;
	md_rd_buf.dev_id = addr;
	md_rd_buf.reg_id = reg;
	md_rd_buf.rwlen = 1;
	md_rd_buf.wbuf = &val;
	md_rd_buf.reslen = &len;
	md_rd_buf.resbuf = buf;

	if (!modbus_status()){
		if (modbus_opt(&md_rd_buf) < 0){
			return -1;
		}

		if (1 == len){
			error_handle(buf[0]);
			return -1;
		}
		
		num = (buf[0] << 8) | buf[1];
		return num;
	}
	return -1;
}

int modbus_status(void)
{
	return modbus_busy;
}

void mb_engine()
{
    uint8_t  buf[RECV_LEN_MAX];
    int32_t  len;
    int      status;
    uint16_t simulator1 = 0, simulator2 = 0;
    uint32_t data_valid = 0;

	md_dev_t md_buf;

	if (0 != aos_queue_new(&md_info_queue, md_info_buf, sizeof(md_info_buf), QUEUE_SIZE_MAX)){
		printf("error:modbus queue new failed!\r\n");
		return ;
	}

    status = aos_mbmaster_rtu_init(&mb_handler, 2, 9600, MB_PAR_NONE);
    if (status != 0) {
        printf("mbmaster init error\n");
        return;
    }

    while (1) {
		if(0 == aos_queue_recv(&md_info_queue, AOS_WAIT_FOREVER, &md_buf, &len)) {
			if (md_buf.rw_bit == MD_WRITE_D){

				status = aos_mbmaster_write_holding_reginster(mb_handler, md_buf.dev_id, md_buf.reg_id, 
														md_buf.rwlen, md_buf.wbuf, md_buf.resbuf, md_buf.reslen);
			} else if (md_buf.rw_bit == MD_READ_D) {
				status = aos_mbmaster_read_holding_reginster(mb_handler, md_buf.dev_id, md_buf.reg_id, 
														md_buf.rwlen, md_buf.resbuf, md_buf.reslen);
			} else if (md_buf.rw_bit == MD_COIL_S) {
				status = aos_mbmaster_write_coil_reginster(mb_handler, md_buf.dev_id, md_buf.reg_id, 
														md_buf.rwlen, md_buf.resbuf, md_buf.reslen);
			} else if (md_buf.rw_bit == MD_COIL_G) {
				status = aos_mbmaster_read_coil_reginster(mb_handler, md_buf.dev_id, md_buf.reg_id, 
														md_buf.rwlen, md_buf.resbuf, md_buf.reslen);
			}

	        if (status == 0) {
	            printf("md recv ok[%d 0x%x]\n", md_buf.dev_id, md_buf.reg_id);
	        } else if (status == MB_RESPOND_EXCEPTION){
	            printf("recv error code [0x%x]\r\n", md_buf.resbuf[0]);
	        } else {
				printf("mdbus error\r\n");
			}			
		}
    }
}

static void md_cmd_routine(char *buf, int len, int argc, char **argv)
{
	int ret = 0;
	int arg1 = strtoul(argv[2], NULL, 10);
    int arg2 = strtoul(argv[3], NULL, 16);
	int arg3 = strtoul(argv[4], NULL, 16);
	uint8_t resbuf[20] = {0};
	uint32_t reslen = 0;
	md_dev_t cli_buf;

	cli_buf.dev_id = (uint8_t)arg1;
	cli_buf.reg_id = (uint16_t)arg2;
	cli_buf.rwlen = (uint16_t)arg3;
	cli_buf.resbuf = resbuf;
	cli_buf.reslen = &reslen;
	
	switch (*(argv[1]))
	{
	case 'r':	   
		printf("arg2 %x \r\n", arg2);
	   ret = modbus_read(cli_buf.dev_id, cli_buf.reg_id, cli_buf.rwlen, cli_buf.resbuf, cli_buf.reslen);
	   aos_cli_printf("read modbus dev [%d][%d] len %d\r\n", cli_buf.dev_id, cli_buf.reg_id, reslen);
	   break;
	case 'w':
	   int arg4 = strtoul(argv[5], NULL, 10);
	   uint16_t wtbuf = (uint16_t)arg4;
	   cli_buf.wbuf = &wtbuf;
	   ret = modbus_write(cli_buf.dev_id, cli_buf.reg_id, cli_buf.rwlen, cli_buf.wbuf, cli_buf.resbuf, cli_buf.reslen);
	   aos_cli_printf("read modbus dev [%d][%d] len %d\r\n", cli_buf.dev_id, cli_buf.reg_id, reslen);
	   break;
	case 's':
		printf("%x %x \r\n", cli_buf.reg_id, cli_buf.rwlen);
	   ret = modbus_set(cli_buf.dev_id, cli_buf.reg_id, cli_buf.rwlen, cli_buf.resbuf, cli_buf.reslen);
	   aos_cli_printf("modbus set dev [%d][%d] len %d\r\n", cli_buf.dev_id, cli_buf.reg_id, reslen);
	   break;
	case 'g':	   
		printf("arg2 %x \r\n", arg2);
	   ret = modbus_get(cli_buf.dev_id, cli_buf.reg_id, cli_buf.rwlen, cli_buf.resbuf, cli_buf.reslen);
	   aos_cli_printf("read modbus dev [%d][%d] len %d\r\n", cli_buf.dev_id, cli_buf.reg_id, reslen);
	   break;
	case 'p':
	   break;

	default:
	   aos_cli_printf("lidar Unknown cs %d\r\n", *(argv[1]));
	   break;
	}

	if (ret < 0){
		printf("modbus opt error\r\n");
	}
}


static struct cli_command md_cmd[] = {
    { "md","control modbusr", md_cmd_routine },
};


int32_t modbus_init(void)
{
	aos_mbmaster_rtu_init(&mb_handler, 2, 9600, MB_PAR_NONE);
	/*
	int32_t rc = -1;
	rc = aos_task_new("modbus", mb_engine, NULL, MODBUS_TASK_DEPTH);
    if (0 != rc) {
		printf("create modbus fail\r\n");
	}

	aos_cli_register_commands(&md_cmd[0], sizeof(md_cmd) / sizeof(struct cli_command));
    return rc;*/
}

